/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: mcl.cpp,v 1.2 2007-06-14 14:58:52 jari Exp $
 *
 */
#include "mcl.h"
#include <time.h>
#include <assert.h>
#include <stdlib.h>

//#include "laserSICK/laserModule.h"
//#include "globalDevs.h"
#include "mcl_interface.h"
//#include "ownutils.h"

//*****************************************************************************
//*****************************************************************************
static int mclThreadFunc(TMCL *mcl);
static int mclDoMCL(TMCL *mcl, const int seq);
//*****************************************************************************
//*****************************************************************************

TMCL *mclCreate()
{
#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Creating MCL!\n");
#endif
  TMCL *mcl = (TMCL *)malloc(sizeof(TMCL));
  assert(mcl != NULL);
  
  if (mclInitialize(mcl) != MCL_OK) {
    free(mcl);
    mcl = NULL;
  }
  
  return mcl;
}
//*****************************************************************************

int mclInitialize(TMCL *mcl)
{
#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Initializing MCL!\n");
#endif

  // Initialize protecting mutex
  pthread_mutex_init(&mcl->mutex, NULL);

  // Do kludge setup
  //mcl->mclDataPtr = reinterpret_cast<void *>(new TMCLData);

  // Do MCL lib init
  //TMCLData *md = reinterpret_cast<TMCLData *>(mcl->mclDataPtr);
  initMCL(mcl->mcld);
#ifdef JB_MCL_DEBUG
  fprintf(stderr,"MCL Initialized, Number of map elements: %d (size=%d)\n",mcl->mcld.map.NumOfLines,mmcl->mcld.map.size); 
#endif
  
  TPose p = { 0, 0, 0, 0 };
  mclSetPose(mcl, &p);

  // Return OK.
  return MCL_OK;
}
//*****************************************************************************

int mclDeinitialize(TMCL *mcl)
{
    
#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Deinitializing MCL\n");
#endif


  mclStop(mcl);
  pthread_mutex_lock(&mcl->mutex);
  pthread_mutex_unlock(&mcl->mutex);
  pthread_mutex_destroy(&mcl->mutex);

  //delete reinterpret_cast<TMCLData *>(mcl->mclDataPtr);

  return MCL_OK;
}
//*****************************************************************************

void mclDestroy(TMCL *mcl)
{
#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Destroying MCL\n");
#endif

  mclDeinitialize(mcl);
  free(mcl);
}
//*****************************************************************************

int mclStart(TMCL *mcl)
{
  int result = MCL_ERROR;

#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Starting MCL\n");
#endif

  pthread_mutex_lock(&mcl->mutex);
  if (mcl->threadActive == 0) {
    // Start the processing Thread
    mcl->threadActive = 1;
    pthread_create(&mcl->thread, NULL, 
		   (void* (*)(void*))mclThreadFunc, 
		   mcl);
    result = MCL_OK;
  } 
  pthread_mutex_unlock(&mcl->mutex);

  return result;
}
//*****************************************************************************

int mclStop(TMCL *mcl)
{
#ifdef JB_MCL_DEBUG
    fprintf(stderr,"Stopping MCL\n");
#endif

  int result = MCL_ERROR;

  if (mcl->threadActive == 1) {
    void *foo;
    mcl->threadActive = 0;
    pthread_join(mcl->thread, &foo);
  }
  
  return result;
}
//*****************************************************************************

int mclGetPose(TMCL *mcl, TPose *pose)
{
  pthread_mutex_lock(&mcl->mutex);
  //TMCLData *md = reinterpret_cast<TMCLData *>(mcl->mclDataPtr);
  pose->x = mcl->mcld.meanPos.x;
  pose->y = mcl->mcld.meanPos.y;
  pose->a = mcl->mcld.meanPos.a;
  pose->time = mcl->mcld.PPD.Lik; ///< The Likelihood of the distribution is stored here!! 
  pthread_mutex_unlock(&mcl->mutex);
  return MCL_OK;
} 
//*****************************************************************************
/**
* Sets the _initial_ pose for the MCL. Must be called once!
*/
int mclSetPose(TMCL *mcl, const TPose *pose)
{
 // TMCLData *md = reinterpret_cast<TMCLData *>(mcl->mclDataPtr);

  

  TPose p;
  pthread_mutex_lock(&mcl->mutex);
  p.x  = pose->x;
  p.y = pose->y;
  p.a = pose->a;
  p.time = 0;

  mcl_initializeDistribution(mcl->mcld, p, NUMBER_OF_PARTICLES);
  mcl->mcld.isInitialized = true;
  pthread_mutex_unlock(&mcl->mutex);
#ifdef JB_MCL_DEBUG
  fprintf(stderr,"MCL Distribution Initialized to '%.1f %.1f %.1f' With %d Particles\n",p.x,p.y,p.a,mcl->mcld.PPD.NumOfParticles);
#endif

  return MCL_OK;
}
//*****************************************************************************


//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

static int mclThreadFunc(TMCL *mcl)
{
  //TLaserNetPacket packet;
  int backoff = 1;
  int result = MCL_OK;
  int lastSeq = 0;
  int timeout_ms = 5000;
  int timeout_count = 0;
  

  
 

  // This function is run in its own thread.
  // IF mclDoMCL returns error, this loop will terminate.
  while(mcl->threadActive && result == MCL_OK) {
    // 1. Read laser & position data.
  
    //lastSeq = laserModuleGetScanEx(&j2b2_global_ptr->laser, 
//				   &packet, lastSeq, timeout_ms);
  
   // 2. Call mclDoMCL to carry out data update
    if (lastSeq > 0 && mcl_isOk(mcl->mcld)){
  
      result = mclDoMCL(mcl, lastSeq);
  
    } else {
      fprintf(stderr,"MCL failure! lastSeq = %d, mcl_isOk = %d.",
	     lastSeq, mcl_isOk(mcl->mcld));
      result = MCL_ERROR;
    }

  }
  
  // Verify.
  mcl->threadActive = 0;

  return result;
}
//*****************************************************************************

static int mclDoMCL(TMCL *mcl, const int seq)
{ 
  //TMCLData *md = reinterpret_cast<TMCLData *>(mcl->mclDataPtr);
  float r[SCANS_IN_SWEEP];
  int i; 
  TPose p;

#ifdef JB_MCL_DEBUG
  clock_t t;
  t =  clock();
#endif
  
  for(i=0;i<SCANS_IN_SWEEP;i++){
    //r[i] =  (float)lp->scan[i]/100.0f; 
  }
  //p.x = lp->x;
  //p.y = lp->y;
  //p.a = lp->angle;
  p.time = 0;
  pthread_mutex_lock(&mcl->mutex);
  calculateMCL(r, p, mcl->mcld);
  
#ifdef JB_MCL_DEBUG
  fprintf(stderr,"t_elapsed: %.2f L='%.2f'\n",(float)(clock()-t)/CLOCKS_PER_SEC,mcl->mcld.PPD.Lik);
#endif
  pthread_mutex_unlock(&mcl->mutex);
  
  return MCL_OK;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
